%注意数据存储方式，是组号- 时间 - 数据类型（加速度速度或者位置）
res = zeros(n_group,length(1:simu_step),3);
motor_res = zeros(n_group,length(1:simu_step),3);
state_info = zeros(n_group,length(1:simu_step),3);
debug_info = zeros(n_group,length(1:simu_step),4);

a_sj.reset_state(initial_state);
pos_pid.pid_reset();
%先把pid架设好
con_value =[0;0];
sample_time_count = sample_period;
for i = 1:vel_simu_step
    if(sample_time_count >= sample_period)
        pos_pid.set_target(vel_s_traj_value(i,:));
        debug_info(:,i,:) = pos_pid.get_con_value(a_sj,a_sj.joint_state,sample_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    motor_res(:,i,:) = tor_motor.simu_loop(a_sj,simu_period,con_value);
    state_info(:,i,:) = (a_sj.simu_loop(motor_res(:,i,1),simu_period));
    sample_time_count = sample_time_count+simu_period;
end


%绘图

figure;
yyaxis left
plot(0:simu_period:(vel_simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(vel_simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(vel_simu_step-1)*simu_period,vel_s_traj_value(1:end-1,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
yyaxis right
plot(0:sample_period:(vel_simu_step-1)*simu_period,debug_info(1,1:(sample_period/simu_period):end-1,3),':o','MarkerIndices',1:1000:simu_step/(sample_period/simu_period),'MarkerSize',2);
hold on
plot(0:sample_period:(vel_simu_step-1)*simu_period,debug_info(2,1:(sample_period/simu_period):end-1,3),'y:o','MarkerIndices',1:1000:(simu_step)/(sample_period/simu_period),'MarkerSize',2);
legend('n=10','n=50','target','n=10 err','n= 50 err','Location',"southeast");
title("vel S traj follow  theta");
figure;
plot(0:simu_period:(vel_simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(vel_simu_step-1)*simu_period,state_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(vel_simu_step-1)*simu_period,vel_s_traj_value(1:end-1,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('n=10','n=50','target');
title("vel S traj follow  omega");
figure;
plot(0:simu_period:(vel_simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(vel_simu_step-1)*simu_period,state_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(vel_simu_step-1)*simu_period,vel_s_traj_value(1:end-1,3),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('n=10','n=50','target');
title("vel S traj follow  acc");